#include <WiFi.h>
#include "Pins.h"
#include "Camera.h"
#include <WebServer.h>

//电机控制引脚
#define IN1 12 
#define IN2 13
#define IN3 14
#define IN4 2
#define PWM 15

#define S 0 //stop
#define U 1
#define D 2
#define L 3
#define R 4
#define UR 5
#define DR 6
#define UL 7
#define DL 8
#define DELAY 10

#ifndef APSSID
#define APSSID "CAR_AP"
#define APPSK  "12345678"
#endif


// OV2640 camera.
Camera ov2640;
WebServer server(8080); 
int dir = 0;//0:stop 1:up 2:r 3:d 4:l
long lastest_CMD_Time = 0;

const char *ssid = APSSID;
const char *password = APPSK;

void handleRoot() //回调函数
{
  if(server.arg(0) == "dir_up"){
      dir = U;
//      forward();
    }
  else if(server.arg(0) == "dir_down"){
      dir = D;
//      backward();
    }
  else if(server.arg(0) == "dir_left"){
      dir = L;
//      left();
    }
  else if(server.arg(0) == "dir_right"){
      dir = R;
//      right();
    }
  else if(server.arg(0) == "dir_up_right"){
    dir = UR;
//      right();
  }
  else if(server.arg(0) == "dir_down_right"){
    dir = DR;
//      backward_right();
  }
  else if(server.arg(0) == "dir_up_left"){
    dir = UL;
//      left();
  }
  else if(server.arg(0) == "dir_down_left"){
    dir = DL;
//      backward_left();
  }
  else if(server.arg(0) == "dir_center"){
    dir = S;
//      car_stop();
  }

  lastest_CMD_Time = millis();
}

void forward()
{
  digitalWrite(IN3,HIGH); 
  digitalWrite(IN4,LOW);
  delay(DELAY);
  Serial.println("' to forward");  
}

void backward()
{
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);
  delay(DELAY);
  Serial.println("' to backward");
}

void left()
{
  
  digitalWrite(IN3,HIGH); 
  digitalWrite(IN4,LOW);
  digitalWrite(IN1,HIGH); 
  digitalWrite(IN2,LOW);
  Serial.println("' to left");
  delay(DELAY);
  
}

void right()
{
  digitalWrite(IN3,HIGH); 
  digitalWrite(IN4,LOW); 
  digitalWrite(IN1, LOW); 
  digitalWrite(IN2, HIGH); 
  Serial.println("' to right");   
  delay(DELAY);  
}

void backward_right()
{
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);
  digitalWrite(IN1,LOW); 
  digitalWrite(IN2,HIGH);
  delay(DELAY);
}

void backward_left()
{
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);
  digitalWrite(IN1,HIGH); 
  digitalWrite(IN2,LOW);
  delay(DELAY);
}

void car_stop()
{
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,LOW);
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,LOW);
  //Serial.println("' to stop");
}

void setup()
{
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  pinMode(IN3,OUTPUT);
  pinMode(IN4,OUTPUT);
  pinMode(PWM,OUTPUT);
  
  Serial.begin(115200);
  ov2640.initialize();

  WiFi.softAP(ssid, password);

  ov2640.startCameraServer();

  server.on("/", handleRoot); 
  server.begin(); //启动服务器
  
  Serial.println("HTTP server started");

  Serial.print("Camera Ready! Use 'http://");
  Serial.print(WiFi.localIP());
  Serial.println("' to connect");
}

void loop()
{
  server.handleClient();

  if (millis() - lastest_CMD_Time > 500)
  {
    dir = S;
  }
  
  switch(dir){
    case S:
      car_stop();
      break;
    case U:
      forward();
      break;
    case D:
      backward();
      break; 
    case L:
      left();
      break; 
    case R:
      right();
      break; 
    case UR:
      right();
      break; 
    case DR:
      backward_right();
      break; 
    case UL:
      left();
      break; 
    case DL:
      backward_left();
      break; 
  }  
}
